#ifndef EM80_CORE_SENSORS_HPP
#define EM80_CORE_SENSORS_HPP

#include "../packet_base.hpp"
#include "../payload_headers.hpp"
#include "../em80_payload_factory.h"
#include "../../v8stdint.h"

#include <memory>

namespace em80_driver
{

class CoreSensors : public EM80PayloadBase
{
public:
    CoreSensors(const EM80PacketPtr &packet) : EM80PayloadBase(packet, "CoreSensors", false, 48) {}

    struct Data
    {
        int32_t  odometry_x;        // unit : mm
        int32_t  odometry_y;        // unit : mm
        int16_t  odometry_th;       // directional angle unit : 0.01deg
        int16_t  linear_vel;        // linear velocity mm/s
        int16_t  imu_th;            // directional angle (?) unit : 0.01 deg
        int16_t  imu_alpha;         // angular velocity  unit : 0.02 deg/s
        int16_t  acc_x;             // acceleration x
        int16_t  acc_y;             // acceleration y
        int16_t  acc_z;             // acceleration z
        uint8_t  bumper;
        uint8_t  cliff;
        uint32_t left_encoder;      // pulses : 0~65535 auto flush
        uint32_t right_encoder;     // pulses : 0~65535 auto flush
        uint16_t ultrasonic_1;      // n >= 1 unit : cm
        uint16_t ultrasonic_2;
        uint16_t ultrasonic_3;
        uint16_t ultrasonic_4;
        uint16_t ultrasonic_5;
        uint16_t ultrasonic_6;
        uint16_t ultrasonic_7;
        uint16_t ultrasonic_8;
    };

    struct Flags
    {
        // bumper
        static const uint8_t FRONTBUMPER = 0x01;
        static const uint8_t BACKBUMPER = 0x02;

        // cliff
        static const uint8_t FRONTCLIFF = 0x01;
        static const uint8_t BACKCLIFF = 0x02;

    };

    Data data;

    ~CoreSensors() {}

    //not to be used directly by external source
    bool serialize()
    {
        // //TODO : is it directly control the raw data
        // std::shared_ptr<Buffer> byteStream = std::make_shared<Buffer>(packet->payload(), packet->packet().second);
        // buildBytes(static_cast<uint8_t>(Header::CoreSensors), *byteStream);
        // buildBytes(static_cast<uint8_t>(length), *byteStream);
        // buildBytes(data.odometry_x, *byteStream);
        // buildBytes(data.odometry_y, *byteStream);
        // buildBytes(data.odometry_th, *byteStream);
        // buildBytes(data.imu_th, *byteStream);
        // buildBytes(data.imu_alpha, *byteStream);
        // buildBytes(data.bumper, *byteStream);
        // buildBytes(data.cliff, *byteStream);
        // buildBytes(data.left_encoder, *byteStream);
        // buildBytes(data.right_encoder, *byteStream);
        // buildBytes(data.ultrasonic_1, *byteStream);
        // buildBytes(data.ultrasonic_2, *byteStream);
        // buildBytes(data.ultrasonic_3, *byteStream);
        // buildBytes(data.ultrasonic_4, *byteStream);
        // buildBytes(data.ultrasonic_5, *byteStream);
        // buildBytes(data.ultrasonic_6, *byteStream);
        // buildBytes(data.ultrasonic_7, *byteStream);
        // buildBytes(data.ultrasonic_8, *byteStream);

        return true;
    }


    bool deserialize()
    {
        std::vector<uint8_t> byteStream;

        byteStream.assign(packet->payload(), packet->payload() + static_cast<uint32_t>(length) + 2);


        //TODO: check them
        uint8_t header_id, length_packed;
        buildVariable(header_id, byteStream);
        buildVariable(length_packed, byteStream);
        if(header_id != Header::CoreSensors) return false;
        if(length_packed != length) return false;
    
        buildVariable(data.odometry_x, byteStream);
        buildVariable(data.odometry_y, byteStream);
        buildVariable(data.odometry_th, byteStream);
        buildVariable(data.linear_vel, byteStream);
        buildVariable(data.imu_th, byteStream);
        buildVariable(data.imu_alpha, byteStream);
        buildVariable(data.acc_x, byteStream);
        buildVariable(data.acc_y, byteStream);
        buildVariable(data.acc_z, byteStream);
        buildVariable(data.bumper, byteStream);
        buildVariable(data.cliff, byteStream);
        buildVariable(data.left_encoder, byteStream);
        buildVariable(data.right_encoder, byteStream);
        buildVariable(data.ultrasonic_1, byteStream);
        buildVariable(data.ultrasonic_2, byteStream);
        buildVariable(data.ultrasonic_3, byteStream);
        buildVariable(data.ultrasonic_4, byteStream);
        buildVariable(data.ultrasonic_5, byteStream);
        buildVariable(data.ultrasonic_6, byteStream);
        buildVariable(data.ultrasonic_7, byteStream);
        buildVariable(data.ultrasonic_8, byteStream);
        return true;
    }
};


} // namespace em80_driver

#endif /* EM80_CORE_SENSORS_HPP*/
